/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package ticks.chromedome;


import AutoCommandGroups.AutoMainCommandGroup;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import ticks.chromedome.commands.CommandBase;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */

//To DO-in any order
//add smartdashboard stuff
//add debugging stuff for
//Elevator
//Potty Chair
//Shooter PID
//SlashPID
//Stopper
//Suspension
//Turret
//VisionPID
//Vision Thread
//Arduino Thread
//add labview dash output
//add DS side vision

public class Robot extends IterativeRobot {

    Command Auto_Main_Command_Group;
    
    
    

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        // instantiate the command used for the autonomous period
        Auto_Main_Command_Group = new AutoMainCommandGroup();

        // Initialize all subsystems
        CommandBase.init();
        
    }

    public void autonomousInit() {
        // schedule the autonomous command (example)
        Auto_Main_Command_Group.start();
        CommandBase.shooter_static.enable();
        
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
        CommandBase.shooter_control.execute();
    }

    public void teleopInit() {
	// This makes sure that the autonomous stops running when
        // teleop starts running. If you want the autonomous to 
        // continue until interrupted by another command, remove
        // this line or comment it out.
        Auto_Main_Command_Group.cancel();
        CommandBase.shooter_static.enable();;
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
        CommandBase.shooter_control.execute();
    }
}
